
#include "models/registration/icp_registration.hpp"
#include "glog/logging.h"
#include "CommonFunc.h"
#include <ros/ros.h>


namespace avp_slam {

    ICPRegistration::ICPRegistration(const YAML::Node &node)
            : icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {

        float max_dis = node["max_dis"].as<float>();
        float trans_eps = node["trans_eps"].as<float>();
        int max_iter = node["max_iter"].as<int>();
        float euclidean_eps = node["euclidean_eps"].as<float>();

        SetRegistrationParam(max_dis, trans_eps, max_iter, euclidean_eps);
    }

    ICPRegistration::ICPRegistration(float max_dis, float trans_eps, int max_iter, float euclidean_eps)
            : icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {

        SetRegistrationParam(max_dis, trans_eps, max_iter, euclidean_eps);
    }

    bool ICPRegistration::SetRegistrationParam(float max_dis, float trans_eps, int max_iter, float euclidean_eps) {
        icp_ptr_->setMaxCorrespondenceDistance(max_dis);
        icp_ptr_->setTransformationEpsilon(trans_eps);
        icp_ptr_->setMaximumIterations(max_iter);
        icp_ptr_->setEuclideanFitnessEpsilon(euclidean_eps);

        LOG(INFO) << "ICP 的匹配参数为：" << std::endl
                  << "max_dis: " << max_dis << ", "
                  << "trans_eps: " << trans_eps << ", "
                  << "max_iter: " << max_iter << ", "
                  << "euclidean_eps: " << euclidean_eps
                  << std::endl << std::endl;

        return true;
    }

    bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR &input_target) {
        icp_ptr_->setInputTarget(input_target);

//        Save _target.pcd
        static int i = 0;
        i++;
        std::string file_name = WORK_SPACE_PATH + "/data/ICP_ScanMatch/" + std::to_string(i);
        pcl::io::savePCDFileASCII(file_name + "_target.pcd", *input_target);

        return true;
    }

    Eigen::Matrix4f ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR &input_source,
                                               const Eigen::Matrix4f &predict_pose,
                                               CloudData::CLOUD_PTR &result_cloud_ptr) {
        ros::Time beginTime = ros::Time::now();

        icp_ptr_->setInputSource(input_source);
        icp_ptr_->align(*result_cloud_ptr, predict_pose);

//        Save _source.pcd && _result.pcd
        static int i = 0;
        i++;
        std::string file_name = WORK_SPACE_PATH + "/data/ICP_ScanMatch/" + std::to_string(i);
        pcl::io::savePCDFileASCII(file_name + "_source.pcd", *input_source);
        pcl::io::savePCDFileASCII(file_name + "_result.pcd", *result_cloud_ptr);

        Eigen::Matrix4f result_pose = icp_ptr_->getFinalTransformation();


//        result_pose = result_pose * predict_pose;

        Eigen::Matrix3f rpy = predict_pose.block(0, 0, 3, 3);
        rpy = result_pose.block(0, 0, 3, 3);
        _hasConverged = icp_ptr_->hasConverged();
        _fitnessScore = icp_ptr_->getFitnessScore(1);

//        int FinalNumIteration = icp_ptr_->getFinalNumIteration();
        if (_hasConverged && _fitnessScore < 0.2) {
            ros::Time endTime = ros::Time::now();
            sumTime = sumTime + (endTime - beginTime).toSec();
            cnt = cnt + 1;
            ROS_INFO("Time consumption: aver %f, cur %f seconds. Iteration %d times. FitnessScore: %f.",
                     GetAverTime(), (endTime - beginTime).toSec(), 999, _fitnessScore);
            return result_pose;
        } else {
            ROS_INFO("ICP has not converged! score %f converged %d", _fitnessScore, _hasConverged);
            return result_pose;
        }

        return result_pose;
    }


    float ICPRegistration::GetAverTime() {
        float re = sumTime / cnt;
        return re;
    }

    float ICPRegistration::GetFitnessScore() {
        return _fitnessScore;
    }

    bool ICPRegistration::GetHasConverged() {
        return _hasConverged;
    }

}